#include "../../../XMLTools.h"
#include "../../../MathTools.h"

#include "WholeBodyDynamicSensorMeasurement.h"

using namespace MMM;

WholeBodyDynamicSensorMeasurement::WholeBodyDynamicSensorMeasurement(float timestep, const Eigen::Vector3f &centerOfMass, const Eigen::Vector3f &angularMomentum, SensorMeasurementType type) :
    InterpolatableSensorMeasurement(timestep, type),
    centerOfMass(centerOfMass),
    angularMomentum(angularMomentum)
{
}

SensorMeasurementPtr WholeBodyDynamicSensorMeasurement::clone() {
    return clone(timestep);
}

bool WholeBodyDynamicSensorMeasurement::equals(SensorMeasurementPtr sensorMeasurement) {
    WholeBodyDynamicSensorMeasurementPtr ptr = boost::dynamic_pointer_cast<WholeBodyDynamicSensorMeasurement>(sensorMeasurement);
    if (ptr) {
        return centerOfMass == ptr->centerOfMass && angularMomentum == ptr->angularMomentum;
    }
    return false;
}

WholeBodyDynamicSensorMeasurementPtr WholeBodyDynamicSensorMeasurement::clone(float newTimestep) {
    WholeBodyDynamicSensorMeasurementPtr clonedSensorMeasurement(new WholeBodyDynamicSensorMeasurement(newTimestep, centerOfMass, angularMomentum, type));
    return clonedSensorMeasurement;
}

void WholeBodyDynamicSensorMeasurement::appendMeasurementDataXML(RapidXMLWriterNodePtr measurementNode) {
    measurementNode->append_node("CenterOfMass")->append_data_node(XML::toString(centerOfMass));
    measurementNode->append_node("AngularMomentum")->append_data_node(XML::toString(angularMomentum));
}

WholeBodyDynamicSensorMeasurementPtr WholeBodyDynamicSensorMeasurement::interpolate(WholeBodyDynamicSensorMeasurementPtr other, float timestep, SensorMeasurementType type) {
    Eigen::Vector3f interpolatedCenterOfMass = Math::linearInterpolation(this->centerOfMass, this->timestep, other->centerOfMass, other->timestep, timestep);
    Eigen::Vector3f interpolatedAngularMomentum = Math::linearInterpolation(this->angularMomentum, this->timestep, other->angularMomentum, other->timestep, timestep);
    WholeBodyDynamicSensorMeasurementPtr interpolatedSensorMeasurement(new WholeBodyDynamicSensorMeasurement(timestep, interpolatedCenterOfMass, interpolatedAngularMomentum, type));
    return interpolatedSensorMeasurement;
}

Eigen::Vector3f WholeBodyDynamicSensorMeasurement::getCenterOfMass() {
    return centerOfMass;
}

Eigen::Vector3f WholeBodyDynamicSensorMeasurement::getAngularMomentum() {
    return angularMomentum;
}
